#define _USE_MATH_DEFINES
#include <cmath>
#include "AlgoEvaluatorModule13Motivation.h"

class algo::AlgoEvaluatorModule13MotivationPrivate
{
public:
};

algo::AlgoEvaluatorModule13Motivation::AlgoEvaluatorModule13Motivation()
	: d(new AlgoEvaluatorModule13MotivationPrivate)
{
}

algo::AlgoEvaluatorModule13Motivation::~AlgoEvaluatorModule13Motivation()
{
	delete d;
}

// Helper function to calculate distance between two lat/lon points in meters using Haversine formula.
// TODO: Move this to a common utility file to avoid duplication.
static double haversine_distance(double lat1, double lon1, double lat2, double lon2) {
    const double R = 6371000.0; // Earth radius in meters
    auto to_radians = [](double degree) {
        return degree * M_PI / 180.0;
    };

    double lat1_rad = to_radians(lat1);
    double lon1_rad = to_radians(lon1);
    double lat2_rad = to_radians(lat2);
    double lon2_rad = to_radians(lon2);

    double dlat = lat2_rad - lat1_rad;
    double dlon = lon2_rad - lon1_rad;

    double a = sin(dlat / 2.0) * sin(dlat / 2.0) +
               cos(lat1_rad) * cos(lat2_rad) *
               sin(dlon / 2.0) * sin(dlon / 2.0);
    double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));

    return R * c;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule13Motivation::getNeedType()
{
	// This module requires ocean current data (U and V components).
	return std::vector<algobase::EvaluateEnvirDataType>({ algobase::EEDT_CS_UO, algobase::EEDT_CS_VO });
}

int algo::AlgoEvaluatorModule13Motivation::exec(const algobase::Path& path, double& result, algobase::EvaluateModuleCoords& indexCoords)
{
	if (path.size() < 2) {
		result = 0.0;
		indexCoords.clear();
		return 0;
	}

	auto db = getDB();
	if (!db) {
		return -1;
	}

	// Drag coefficient 'c' from the base energy calculation. Should be consistent.
	const double drag_coeff = 20.0;

	double total_extra_energy_joules = 0.0;
	indexCoords.resize(path.size());
	indexCoords[0].Assign(path[0]);
	indexCoords[0].mResult = 0.0;

	for (size_t i = 0; i < path.size() - 1; ++i) {
		const auto& p1 = path[i];
		const auto& p2 = path[i+1];

		// --- Calculate segment properties ---
		double h_dist = haversine_distance(p1.mCoordZ[1], p1.mCoordZ[0], p2.mCoordZ[1], p2.mCoordZ[0]);
		double v_dist = std::abs(p2.mCoordZ[2] - p1.mCoordZ[2]);
		double segment_dist_m = std::sqrt(h_dist * h_dist + v_dist * v_dist);
		
		double speed_mps = p1.mSpeed;
		double segment_time_s = (speed_mps > 1e-6) ? (segment_dist_m / speed_mps) : 0.0;

		if (segment_dist_m < 1e-6) {
			indexCoords[i+1].Assign(p2);
			indexCoords[i+1].mRawValue = total_extra_energy_joules;
			indexCoords[i+1].mResult = 0.0;
			continue;
		}

		// --- Calculate extra energy due to currents ---
		// Get current data at the start of the segment
		double u_current = db->getData(algobase::EEDT_CS_UO, getTaskConfig().mStartTime, p1.mCoordZ, 0.0f);
		double v_current = db->getData(algobase::EEDT_CS_VO, getTaskConfig().mStartTime, p1.mCoordZ, 0.0f);

		// Vehicle's velocity vector (relative to ground)
		double heading_rad = p1.mHeading * M_PI / 180.0;
		double vx_vehicle = speed_mps * sin(heading_rad); // U component
		double vy_vehicle = speed_mps * cos(heading_rad); // V component

		// Vehicle's velocity relative to water
		double vx_relative = vx_vehicle - u_current;
		double vy_relative = vy_vehicle - v_current;
		double speed_relative = std::sqrt(vx_relative * vx_relative + vy_relative * vy_relative);

		// Power = c * v^3. We use speed relative to water for actual power consumption.
		double power_actual = drag_coeff * std::pow(speed_relative, 3);
		// Base power is what would be needed in still water.
		double power_base = drag_coeff * std::pow(speed_mps, 3);
		
		double power_extra = power_actual - power_base;
		double energy_extra_segment = power_extra * segment_time_s;

		total_extra_energy_joules += energy_extra_segment;

		// Store the INSTANTANEOUS extra energy for the segment at the start point.
		indexCoords[i].Assign(p1);
		indexCoords[i].mRawValue = energy_extra_segment;
		indexCoords[i].mResult = calcScore(energy_extra_segment);
	}
	// Last point has no outgoing segment.
	indexCoords.back().Assign(path.back());
	indexCoords.back().mResult = 0.0;

	// The final raw value is the total extra energy consumption (can be negative if currents are favorable).
	result = calcScore(total_extra_energy_joules);

	return 0;
}

double algo::AlgoEvaluatorModule13Motivation::calcScore(double rawValue) const
{
	double refEnergy_J = 1e7;       // Typical segment energy in Joules
	return costSigmoid(rawValue, refEnergy_J * 0.1, refEnergy_J * 0.05) * getWeight();
}

double algo::AlgoEvaluatorModule13Motivation::calcRaw(const std::vector<double>& rawValues) const
{
	double total_extra_energy_joules = 0.0;
	for (const auto& val : rawValues) {
		total_extra_energy_joules += val;
	}
	return total_extra_energy_joules;
}
